var teamFlight = require('cylon');
var keypress = require('keypress');

var dispText = "Drone Command Received: ";
var dispText2 = "Drone Status Update   : ";
var droneIP = '192.168.1.1';

var flightTime = 0;

var throttle = 0;
var forwardAccRate = .1;
var backwardAccRate = .1;
var rightAccRate = .1;
var leftAccRate = .1;
var rotateRightAccRate = .6;
var rotateLeftAccRate = .6;
var upAccRate = .3;
var downAccRate = .3;



teamFlight.robot({
	connections: 
	{
		//loopback: { adaptor: 'loopback' },
		ardrone: { adaptor: 'ardrone', port: droneIP }
	},

	devices: 
	{    
		drone: { driver: 'ardrone' },
		nav: { driver: 'ardrone-nav' },
	},


	//Work Loop - Continues Running Until Program Exits
	work: function(droneObj) {
		
		//Reports drone status updates (currently not working)
		droneObj.drone.config('general:navdata_demo', 'TRUE');
		droneObj.nav.on('update', function(data) {
		  console.log(data);
		});

		//Captures keyboard input
		keypress(process.stdin);
		process.stdin.on('keypress', function (ch, key) {
			//console.log('Received Command: "keypress"', key);

			try {
				if (key.sequence == 'w' || key.name == 'up'){
					droneObj.moveForward(droneObj);
				}
				if (key.sequence == 's'|| key.name == 'down'){
					droneObj.moveBackward(droneObj);
				}
				if (key.sequence == 'd'|| key.name == 'right'){
					droneObj.moveRight(droneObj);
				}
				if (key.sequence == 'a'|| key.name == 'left'){
					droneObj.moveLeft(droneObj);
				}
				if (key.sequence == 'e'){
					droneObj.rotateRight(droneObj);
				}
				if (key.sequence == 'q'){
					droneObj.rotateLeft(droneObj);
				}
				if (key.sequence == 'r'){
					droneObj.moveUpInc(droneObj);
				}
				if (key.sequence == 'f'){
					droneObj.moveDownInc(droneObj);
				}
				if (key.sequence == 'p'){
					droneObj.droneTakeOff(droneObj);
				}
				if (key.sequence == 'l'){
					droneObj.droneLand(droneObj);
				}
				if (key.sequence == 'b' || key.name == 'space'){
					droneObj.droneStop(droneObj);
				}
				if (key.sequence == 'h'){
					droneObj.droneHover(droneObj);
				}
				if (key.sequence == 'c'){
					droneObj.droneThrottle(.1);
				}
				if (key.sequence == 'z'){
					droneObj.droneThrottle(-.1);
				}
				if (key.sequence == 'x'){
					droneObj.droneThrottle(0);
				}
				if (key.sequence == 'u'){
					//droneObj.drone.animate('flipBehind',5);
				}
				if (key.name == 'home'){
					droneObj.moveForward(droneObj);
					droneObj.moveLeft(droneObj);
				}
				if (key.name == 'pageup'){
					droneObj.moveForward(droneObj);
					droneObj.moveRight(droneObj);
				}
				if (key.name == 'end'){
					droneObj.moveBackward(droneObj);
					droneObj.moveLeft(droneObj);
				}
				if (key.name == 'pagedown'){
					droneObj.moveBackward(droneObj);
					droneObj.moveRight(droneObj);
				}
				if (key.name == 'undefined'){
					droneObj.droneStop(droneObj);
				}
			}
			catch (error) {
				//console.log("     Invalid command", error);
			}
		});
		process.stdin.setRawMode(true);

		
		//Inner Timed Loops
		every((60).second(), function() {
			console.log("       " + dispText2 + "60 Seconds of Flight Time Elapsed");
		});

		every((1).second(), function() {
			flightTime = flightTime + 1;
		});

	}, //end work function


 
  
	// Command Functions
	 moveForward: function(droneRef) {
		droneRef.drone.front(forwardAccRate + throttle);
		console.log("     " + dispText + "(" + (100 * (forwardAccRate + throttle)) + "%) Forward");
	},

	moveBackward: function(droneRef) {
		droneRef.drone.back(backwardAccRate + throttle);
		console.log("     " + dispText + "(" + (100 * (backwardAccRate + throttle)) + "%) Backward");
	},

	moveRight: function(droneRef) {
		droneRef.drone.right(rightAccRate + throttle);
		console.log("     " + dispText + "(" + (100 * (rightAccRate + throttle)) + "%) Right");
	},

	moveLeft: function(droneRef) {
		droneRef.drone.left(leftAccRate + throttle);
		console.log("     " + dispText + "(" + (100 * (leftAccRate + throttle)) + "%) Left");
	},

	rotateRight: function(droneRef) {
		droneRef.drone.clockwise(rotateRightAccRate);
		console.log("     " + dispText + "(" + (100 * (rotateRightAccRate + 0)) + "%) Right Rotate");
	},

	rotateLeft: function(droneRef) {
		droneRef.drone.counterClockwise(rotateLeftAccRate);
		console.log("     " + dispText + "(" + (100 * (rotateLeftAccRate + 0)) + "%) Left Rotate");
	},  

	moveUpInc: function(droneRef) {
		droneRef.drone.up(upAccRate);
		after((2).second(), function() {
			droneRef.droneStop(droneRef);
		});
		console.log("     " + dispText + "(" + (100 * upAccRate) + "%) Up");
	},

	moveDownInc: function(droneRef) {
		droneRef.drone.down(downAccRate);
		after((2).second(), function() {
			droneRef.droneStop(droneRef);
		});
		console.log("     " + dispText + "(" + (100 * downAccRate) + "%) Down");
	},	
		
	droneTakeOff: function(droneRef) {
		droneRef.drone.takeoff();
		console.log("     " + dispText + "Take Off");
		flightTime = 0;
	},

	droneLand: function(droneRef) {
		droneRef.drone.land();
		console.log("     " + dispText + "Landing - Flight Time: " + flightTime + " seconds");
		flightTime = 0;
	},
	  
	droneStop: function(droneRef) {
		droneRef.drone.stop();
		console.log("     " + dispText + "Stop and Reset");
	},  

	droneHover: function(droneRef) {
		droneRef.drone.hover();
		console.log("     " + dispText + "Hovering");
	},  

	droneThrottle: function(accFactor) {
		if (accFactor == 0)	throttle = 0;
		else {
			throttle = throttle + accFactor;
			
			if (throttle < 0) throttle = 0;
			if (throttle > 1) throttle = 1;
		}
		console.log("     " + dispText + "Current Acceleration Rate: " + throttle);
	},  

	

}).start();













